import rclpy
from rclpy.node import Node
from nav_msgs.msg import Path
from geometry_msgs.msg import Pose2D, PoseStamped
from std_msgs.msg import Bool
import math

class PathToActCmdNode(Node):
    def __init__(self):
        super().__init__('path_to_act_cmd_node')

        # 创建订阅者，用于接收路径消息
        self.path_subscriber = self.create_subscription(
            Path,
            'path',
            self.path_callback,
            10
        )

        # 创建发布者，用于发送 act_cmd 消息
        self.act_cmd_publisher = self.create_publisher(
            Pose2D,
            'act_cmd',
            10
        )

        # 创建订阅者，用于接收反馈消息
        self.feedback_subscriber = self.create_subscription(
            Bool,
            'feedback',
            self.feedback_callback,
            10
        )

        # 存储路径数据
        self.path_data = None
        # 当前处理的路径点索引
        self.current_index = 0

    def path_callback(self, msg):
        self.get_logger().info('Received a path message.')
        # 存储路径数据
        self.path_data = msg
        # 重置路径点索引
        self.current_index = 0

        if self.path_data.poses:
            self.process_next_path_point()
        else:
            self.get_logger().warn('Received empty path.')

    def process_next_path_point(self):
        if self.path_data and self.current_index < len(self.path_data.poses):
            pose_stamped = self.path_data.poses[self.current_index]
            pose = pose_stamped.pose

            # 将路径点转换为 act_cmd 消息
            act_cmd_msg = Pose2D()
            act_cmd_msg.x = pose.position.x
            act_cmd_msg.y = pose.position.y
            # 计算目标角度（假设目标角度为姿态的 z 分量）
            orientation = pose.orientation
            act_cmd_msg.theta = 2 * math.atan2(orientation.z, orientation.w)

            # 发布 act_cmd 消息
            self.act_cmd_publisher.publish(act_cmd_msg)
            self.get_logger().info(f'Publishing act_cmd for path point {self.current_index}.')

        else:
            self.get_logger().info('All path points processed.')

    def feedback_callback(self, msg):
        if msg.data:
            self.get_logger().info(f'Feedback received for path point {self.current_index}.')
            # 处理下一个路径点
            self.current_index += 1
            self.process_next_path_point()
        else:
            self.get_logger().warn('Feedback received: false. Waiting for correct feedback.')

def main(args=None):
    rclpy.init(args=args)
    node = PathToActCmdNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
